#include "MIRMM.h"
#include "SignalCaracterizerModule.h"

/* Kernel includes. */
#include "FreeRTOS.h"
#include "task.h"
#include "queue.h"
#include "semphr.h"

#include "LED3.h"
#include "AS1.h"
#include "utilidades.h"

extern xSemaphoreHandle xNewMeasureDurationSemaphore;
xSemaphoreHandle xNewMotionIntentionSemaphore;

static int16_t iGripShape; 
static int16_t iApertureRatio;
static int16_t iForce;

void vTaskCode3( void * pvParameters )
{
  uint16_t uiCntSamples = 0;
  int16_t iSampleFrecuency = 0;
  const portTickType xDelay = 1000 / portTICK_RATE_MS;   
  pvParameters = pvParameters;
  char msg[23];
  uint16_t size = 23;
  vSemaphoreCreateBinary( xNewMotionIntentionSemaphore );
   
     for( ;; )
     {
        xSemaphoreTake( xNewMeasureDurationSemaphore, portMAX_DELAY );
        GetDuration( &uiCntSamples, &iSampleFrecuency );        
        
        /*
        if(uiCntSamples >= 6) 
        {
          iApertureRatio = 100;
          msg = "Hand -> open          ";
          //AS1_SendBlock(msg, sizeof(msg), &size );          
          xSemaphoreGive( xNewMotionIntentionSemaphore );
        }
        else*/ 
        if(uiCntSamples >= 1)        
        {
          if(iApertureRatio == 100)
          {
            iApertureRatio = 10;  
          }
          else
          {
            iApertureRatio = 100;
          }
          //msg = "Hand -> close         ";
          msg = "Hand -> Toggle        ";
          AS1_SendBlock(msg, sizeof(msg), &size );          
          xSemaphoreGive(xNewMotionIntentionSemaphore);
        }
        else
        {
          msg = "Noise                 ";
          //AS1_SendBlock(msg, sizeof(msg), &size );          
        }
        
        LED3_NegVal();
        //AS1_SendChar(0x0A);           
     }
}
  
bool GetHandGrip( int16_t * piGripShape, int16_t * piApertureRatio, int16_t * piForce )
{
  *piGripShape = iGripShape;
  *piApertureRatio = iApertureRatio;
  *piForce = iForce;
}